using System;
using DemoContentLoader;
using DemoRenderer;
using BepuPhysics;
using BepuPhysics.Collidables;
using BepuPhysics.Constraints;
using DemoUtilities;
using DemoRenderer.UI;
#if MYCODE
using BepuUtilities.Vectors;
#else
using System.Numerics;
#endif
namespace Demos.Demos
{
    public class ContinuousCollisionDetectionDemo : Demo
    {
        ConstraintHandle spinnerMotorA;
        ConstraintHandle spinnerMotorB;
        RolloverInfo rolloverInfo;

        ConstraintHandle BuildSpinner(Vector3 initialPosition, float rotationSpeed)
        {
            var spinnerBase = Simulation.Bodies.Add(BodyDescription.CreateDynamic(initialPosition, new BodyInertia { InverseMass = 1e-2f }, new CollidableDescription(Simulation.Shapes.Add(new Box(2, 2, 2)), 0.1f), new BodyActivityDescription(0.01f)));
            var bladeShape = new Box(5, 0.01f, 1);
            bladeShape.ComputeInertia(1, out var bladeInertia);
            var shapeIndex = Simulation.Shapes.Add(bladeShape);
            // 注意,在1e-4处,最小行进持续时间和扫描收敛持续时间都非常小。
            // 这将以等于10000 hz的更新速率的精度检测冲突。
            // 叶片非常薄,旋转速度非常快,所以这种精确度是有帮助的。
            // 请注意,您可能会获得较大的扫掠收敛持续时间。
            // 扫描收敛持续时间是允许扫描终止的"首次撞击时间"区域的最大尺寸;
            // 使用稍微短一点的撞击时间通常不会造成太大的问题。
            // 保持较小的最小行进持续时间要重要得多,因为持续时间低于最小行进持续时间的冲突可能被完全忽略。

            // 请注意,在某些转角情况下,刀片仍有可能相互穿透-CCD扫描只检测到"第一次"撞击的时间。
            // 与第一次碰撞相关联的接触可能不足以在同一帧内进行随后的碰撞。
            // 不过,这是相当罕见的-如果您有一种情况下,这种类型的故障是常见的,考虑增加Collible的投机利润或使用更高的更新率。
            // (我们并不总是依赖于大的投机性边际的原因是幽灵碰撞-投机性接触可能并不代表碰撞
            // 这确实会发生,但无论如何都会包含在约束解决方案中。它们相当罕见,但需要注意。)
            var spinnerBlade = Simulation.Bodies.Add(BodyDescription.CreateDynamic(initialPosition, bladeInertia, new CollidableDescription(shapeIndex, 0.2f, ContinuousDetectionSettings.Continuous(1e-4f, 1e-4f)), new BodyActivityDescription(0.01f)));
            Simulation.Solver.Add(spinnerBase, spinnerBlade, new Hinge { LocalHingeAxisA = new Vector3(0, 0, 1), LocalHingeAxisB = new Vector3(0, 0, 1), LocalOffsetB = new Vector3(0, 0, -3), SpringSettings = new SpringSettings(30, 1) });
            Simulation.Solver.Add(spinnerBase, spinnerBlade, new AngularAxisMotor { LocalAxisA = new Vector3(0, 0, 1), Settings = new MotorSettings(10, 1e-4f), TargetVelocity = rotationSpeed });
            return Simulation.Solver.Add(spinnerBase, new OneBodyLinearServo { ServoSettings = ServoSettings.Default, SpringSettings = new SpringSettings(30, 1) });
        }

        public override void Initialize(ContentArchive content, Camera camera)
        {
            camera.Position = new Vector3(0, 10, 40);
            camera.Yaw = 0;
            camera.Pitch = 0;
            // 请注意,此演示的触点刚度较高。对于演示时间步长默认值为60 hz的一般稳定性而言,这并不理想,但是
            // 这个演示没有任何明显的解算器复杂性,我们希望更清楚地看到CCD的动作-这意味着更严格的接触。
            // 让物体在撞击时反弹一束会让它更难看清。
            // 另请注意,PositionFirstTimestpper是最简单的时步模式,但由于它将速度积分到帧开始处的位置,因此直接修改了时间步长之外的速度
            // 将在碰撞检测或解算器有机会介入之前集成。这在这个演示中很好。其他内置选项包括PositionLastTimestpper和SubsteppingTimestpper。
            // 请注意,timeSteper还具有回调,您可以使用这些回调在处理阶段之间执行逻辑,如BeforeCollisionDetection。
            Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks() { ContactSpringiness = new SpringSettings(120, 1) }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper());

            var shape = new Box(1, 1, 1);
            shape.ComputeInertia(1, out var inertia);
            var shapeIndex = Simulation.Shapes.Add(shape);
            for (int i = 0; i < 10; ++i)
            {
                for (int j = 0; j < 10; ++j)
                {
                    // 这两个下跌的动力都有相当小的投机利润率。第二种方法使用连续的碰撞检测扫描来生成推测性接触。
                    Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(-4 - 2 * j, 100 + (i + j) * 2, i * 2), new BodyVelocity { Linear = new Vector3(0, -150, 0) }, inertia,
                        new CollidableDescription(shapeIndex, 0.01f), new BodyActivityDescription(0.01f)));
                    // 在1e-3处的最小进程持续时间参数意味着CCD扫描将不会错过持续至少1e-3个单位时间的任何冲突-因此,如果时间是以秒来测量的,
                    // 然后,这将捕获更新速率为1000 Hz时会发生的任何冲突。
                    // 还要注意的是,扫描会聚阈值在100 Hz时实际上相当松散。尽管如此,它仍然可以导致相当好的投机性接触和坚实的冲击行为。
                    // 这是因为扫描不会直接生成触点-它会生成估计的撞击时间,然后生成离散触点
                    // 运行以创建实际的触点歧管。这提供了高质量的接触头寸和投机深度。
                    // 如果这些长方体撞击的地面类似于网格-无限薄-您可能希望提高扫描精度。
                    Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(4 + 2 * j, 100 + (i + j) * 2, i * 2), new BodyVelocity { Linear = new Vector3(0, -150, 0) }, inertia,
                        new CollidableDescription(shapeIndex, 0.01f, ContinuousDetectionSettings.Continuous(1e-3f, 1e-2f)), new BodyActivityDescription(0.01f)));
                }
            }
            rolloverInfo = new RolloverInfo();
            rolloverInfo.Add(new Vector3(-12, 2, 0), "Discrete");
            rolloverInfo.Add(new Vector3(12, 2, 0), "Continuous");

            // 构建两个微调器以相互撞击以展示角度CCD。请注意,旋转速度略有不同-这有助于避免
            // 同步使得刀片式服务器经常错失彼此,这有点破坏了CCD演示。
            spinnerMotorA = BuildSpinner(new Vector3(-5, 10, -5), 53);
            spinnerMotorB = BuildSpinner(new Vector3(5, 10, -5), 59);
            rolloverInfo.Add(new Vector3(0, 12, -5), "High angular velocity continuous detection");

            Simulation.Statics.Add(new StaticDescription(new Vector3(0, -5f, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(300, 10, 300)), 0.1f)));
        }

        double time;
        public override void Update(Window window, Camera camera, Input input, float dt)
        {
            // 快速转动旋转器。
            var servo = new OneBodyLinearServo { ServoSettings = ServoSettings.Default, SpringSettings = new SpringSettings(30, 1) };
            servo.Target = new Vector3(-5 - 3.5f * (float)Math.Sin(time), 10, -5);
            Simulation.Solver.ApplyDescription(spinnerMotorA, servo);
            servo.Target = new Vector3(5 + 3.5f * (float)Math.Sin(time), 10, -5);
            Simulation.Solver.ApplyDescription(spinnerMotorB, servo);
            time += dt;
            base.Update(window, camera, input, dt);
        }

        public override void Render(Renderer renderer, Camera camera, Input input, TextBuilder text, Font font)
        {
            rolloverInfo.Render(renderer, camera, input, text, font);
            base.Render(renderer, camera, input, text, font);
        }
    }
}
